//
// Created by yephiu on 2023/7/7.
//

#include "Nizhou_drv_T265.h"
#include "stm32f4xx_hal.h"
#include "Ano_FcData.h"

T265_data_t T265_data; //全局变量 用于储存数据

T265_DataPacket t265DataPacket; //声明数据包
T265_ParserState t265current_state = T265_WAIT_HEADER; //状态机状态位
uint8_t float_data_bytes_received = 0; //浮点数暂存位

uint8_t T265_STATUS=T265_OFFLINE;

uint8_t T265_Init(void)
{
    T265_STATUS=T265_TESTING;
    for (uint8_t i=100;i<100;i++)
    {
        if(T265_STATUS==T265_READY)
        {
            break;
        }
        else
        {
            HAL_Delay(10);//在1秒里面检查T265是否在线
        }
    }
    if(T265_STATUS==T265_TESTING)
    {
        T265_STATUS=T265_OFFLINE;
    }
    sens_hd_check.t265_ok=T265_STATUS;
    return T265_STATUS;
}

//校验和计算
//校验和是异或校验：类型帧^长度帧^数据部分
uint8_t T265_calculateChecksum(T265_DataPacket *packet)
{
    uint8_t *byte_ptr = (uint8_t *)packet->float_data;
    uint8_t checksum = packet->frame_type ^ packet->data_length;

    for (int i = 0; i < 4 * sizeof(float); ++i)
    {
        checksum ^= byte_ptr[i];
    }

    return checksum & 0xFF;
}

//数据解包分析任务
/**
 *
 * @param received_byte
 */
void T265_DataAnlTask(uint8_t received_byte)
{
    switch (t265current_state)
    {
        case T265_WAIT_HEADER:
            if (received_byte == 0x51)
            {
                t265current_state = T265_WAIT_TYPE;
            }
            break;
        case T265_WAIT_TYPE:
            t265DataPacket.frame_type = received_byte;//接收帧类型
            t265current_state = T265_WAIT_LENGTH;
            break;
        case T265_WAIT_LENGTH:
            t265DataPacket.data_length = received_byte;//接收数据长度
            t265current_state = T265_WAIT_FLOAT_DATA;
            break;
        case T265_WAIT_FLOAT_DATA:
            ((uint8_t *)t265DataPacket.float_data)[float_data_bytes_received++] = received_byte;//解析浮点数数据
            if (float_data_bytes_received >= 4 * sizeof(float))
            {
                float_data_bytes_received = 0;
                t265current_state = T265_WAIT_CHECKSUM;
            }
            break;
        case T265_WAIT_CHECKSUM:
            t265DataPacket.checksum = received_byte;//接收检验和
            t265current_state = T265_WAIT_TAIL;
            break;

        case T265_WAIT_TAIL:
            if (received_byte == 0xff)
            {
                if (T265_calculateChecksum(&t265DataPacket) == t265DataPacket.checksum)//满足校验条件
                {
                    if(t265DataPacket.frame_type == 0x01)//四元数帧
                    {
                        T265_data.qx=t265DataPacket.float_data[0];
                        T265_data.qy=t265DataPacket.float_data[1];
                        T265_data.qz=t265DataPacket.float_data[2];
                        T265_data.qw=t265DataPacket.float_data[3];
                    }
                    if(t265DataPacket.frame_type == 0x02)//位置帧数
                    {
                        T265_data.px=t265DataPacket.float_data[0];
                        T265_data.py=t265DataPacket.float_data[1];
                        T265_data.pz=t265DataPacket.float_data[2];
                    }
                    if(t265DataPacket.frame_type == 0x03)//速度帧
                    {
                        T265_data.vx=t265DataPacket.float_data[0];
                        T265_data.vy=t265DataPacket.float_data[1];
                        T265_data.vz=t265DataPacket.float_data[2];
                    }
                    if(t265DataPacket.frame_type == 0x04)//加速度帧
                    {
                        T265_data.ax=t265DataPacket.float_data[0];
                        T265_data.ay=t265DataPacket.float_data[1];
                        T265_data.az=t265DataPacket.float_data[2];
                    }
                    // 数据包解析成功，可以在这里处理接收到的数据包
                }
                //若数据接收正常则说明传感器正常
                sens_hd_check.t265_ok=T265_READY;
            }
            t265current_state = T265_WAIT_HEADER;
            break;
    }
}

T265_flight_dir_t T265_flight_dir;

/**
 *
 * @param dT_ms
 */
void T265_DataUpdateTask(uint8_t dT_ms)
{
    //T265镜头朝向前放置
    //机身坐标：机头为x正，左侧为y正，天为z正
    T265_flight_dir.PZ=T265_data.py;
    T265_flight_dir.PX=-T265_data.pz;
    T265_flight_dir.PY=-T265_data.px;

    T265_flight_dir.VZ=T265_data.vy;
    T265_flight_dir.VX=-T265_data.vz;
    T265_flight_dir.VY=-T265_data.vx;

    T265_flight_dir.PZ=T265_data.ay;
    T265_flight_dir.AX=-T265_data.az;
    T265_flight_dir.AY=-T265_data.ax;

}
